#include <stdio.h>
#include <stdlib.h>
#include <fcntl.h>
#include <stdint.h> 
#include "mpu6050.h"
#include "iic.h"
#include <unistd.h>
#include "linux/i2c-dev.h"
#include <sys/ioctl.h>
#include <sys/time.h>
#include "math.h"

 
float gyro_offset_x,gyro_offset_y,gyro_offset_z;
float aac_offset_x, aac_offset_y, aac_offset_z;
float pitch, roll, yaw, yaw_bef; 		//欧拉角
short aacx, aacy, aacz;		//加速度传感器原始数据
short gyrox, gyroy, gyroz;	//陀螺仪原始数据
short temp;					//温度
float Gyro_Pitch, Gyro_Roll, Gyro_Yaw;//角速度
float Acc_Pitch, Acc_Roll, Acc_Yaw;//加速度
double Angle_Pitch = 0, Angle_Roll = 0, Angle_Yaw = 0;//角度
double last_Seconds = 0.0;
#define YAW_min_move 0.01f //去零漂最小幅度
 
u8 MPU_Write_Byte(u8 reg,u8 data) 				 
{
  iic_write(reg,data);
  return 0;
}
 
u8 MPU_Read_Byte(u8 reg)
{
	return iic_read(reg);		
}

//IIC连续读
//addr:器件地址
//reg:要读取的寄存器地址
//len:要读取的长度
//buf:读取到的数据存储区
//返回值:0,正常
//    其他,错误代码
u8 MPU_Read_Len(u8 addr,u8 reg,u8 len,u8 *buf) {
	if (i2c_read(iicfd, 0, reg, buf, len) == 0) {
		return 0;
	} else {
		return 1;
	}
}
 
 
//初始化MPU6050
//返回值:0,成功
//    其他,错误代码
void  MPU_Init(void)
{ 
	u8 res;
	res = ioctl(iicfd,I2C_TENBIT,0);   //7bit
	if(res != -1) printf("iic 7 bit OK \n");
    res = ioctl(iicfd,I2C_SLAVE,MPU_ADDR);    //设置I2C从设备地址[6:0]
	if(res != -1) printf("iic divice addr set OK \n");
 
	MPU_Write_Byte(MPU_PWR_MGMT1_REG,0X80);	//复位MPU6050
	sleep(1);
	MPU_Write_Byte(MPU_PWR_MGMT1_REG,0X00);	//唤醒MPU6050
	MPU_Set_Gyro_Fsr(3);					//陀螺仪传感器,±2000dps
	MPU_Set_Accel_Fsr(0);					//加速度传感器,±2g
	MPU_Set_Rate(1000);						//设置采样率
	MPU_Write_Byte(MPU_INT_EN_REG,0X00);	//关闭所有中断
	MPU_Write_Byte(MPU_USER_CTRL_REG,0X00);	//I2C主模式关闭
	MPU_Write_Byte(MPU_FIFO_EN_REG,0X00);	//关闭FIFO
	MPU_Write_Byte(MPU_INTBP_CFG_REG,0X80);	//INT引脚低电平有效
	res=MPU_Read_Byte(MPU_DEVICE_ID_REG);
	printf("id = 0x%x \n",res);
	if(res==0x68)//器件ID正确
	{
		MPU_Write_Byte(MPU_PWR_MGMT1_REG,0X01);	//设置CLKSEL,PLL X轴为参考
		MPU_Write_Byte(MPU_PWR_MGMT2_REG,0X00);	//加速度与陀螺仪都工作
		MPU_Set_Rate(50);						//设置采样率为50Hz
		printf("MPU6050 set OK\n");
 	}
	else{
		printf("MPU6050 not OK \n");
	}
}

//得到温度值
//返回值:温度值(扩大了100倍)
short MPU_Get_Temperature(void)
{
    u8 buf[2]; 
    short raw;
	float temp;
	buf[0]=MPU_Read_Byte(MPU_TEMP_OUTH_REG);
	buf[1]=MPU_Read_Byte(MPU_TEMP_OUTH_REG+1);
    raw=((u16)buf[0]<<8)|buf[1];  
    temp=36.53+(double)(raw)/340.0;
    return temp*100;
}

//设置MPU6050陀螺仪传感器满量程范围
//fsr:0,±250dps;1,±500dps;2,±1000dps;3,±2000dps
//返回值:0,设置成功
//    其他,设置失败 
u8 MPU_Set_Gyro_Fsr(u8 fsr)
{
	return MPU_Write_Byte(MPU_GYRO_CFG_REG,fsr<<3);//设置陀螺仪满量程范围  
}
//设置MPU6050加速度传感器满量程范围
//fsr:0,±2g;1,±4g;2,±8g;3,±16g
//返回值:0,设置成功
//    其他,设置失败 
u8 MPU_Set_Accel_Fsr(u8 fsr)
{
	return MPU_Write_Byte(MPU_ACCEL_CFG_REG,fsr<<3);//设置加速度传感器满量程范围  
}
//设置MPU6050的数字低通滤波器
//lpf:数字低通滤波频率(Hz)
//返回值:0,设置成功
//    其他,设置失败 
u8 MPU_Set_LPF(u16 lpf)
{
	u8 data=0;
	if(lpf>=188)data=1;
	else if(lpf>=98)data=2;
	else if(lpf>=42)data=3;
	else if(lpf>=20)data=4;
	else if(lpf>=10)data=5;
	else data=6; 
	return MPU_Write_Byte(MPU_CFG_REG,data);//设置数字低通滤波器  
}
//设置MPU6050的采样率(假定Fs=1KHz)
//rate:4~1000(Hz)
//返回值:0,设置成功
//    其他,设置失败 
u8 MPU_Set_Rate(u16 rate)
{
	u8 data;
	if(rate>1000)rate=1000;
	if(rate<4)rate=4;
	data=1000/rate-1;
	data=MPU_Write_Byte(MPU_SAMPLE_RATE_REG,data);	//设置数字低通滤波器
 	return MPU_Set_LPF(rate/2);	//自动设置LPF为采样率的一半
}
 
//得到陀螺仪值(原始值)
//gx,gy,gz:陀螺仪x,y,z轴的原始读数(带符号)
//返回值:0,成功
//    其他,错误代码
u8 MPU_Get_Gyroscope(short *gx,short *gy,short *gz)
{
	//printf("read %x\n", MPU_Read_Byte(MPU_GYRO_CFG_REG) >> 3);
  u8 buf[6];  
  MPU_Read_Len(0, MPU_GYRO_XOUTH_REG, 6, buf);
	// buf[0]=MPU_Read_Byte(MPU_GYRO_XOUTH_REG);
	// buf[1]=MPU_Read_Byte(MPU_GYRO_XOUTH_REG+1);
	// buf[2]=MPU_Read_Byte(MPU_GYRO_YOUTH_REG);
	// buf[3]=MPU_Read_Byte(MPU_GYRO_YOUTH_REG+1);
	// buf[4]=MPU_Read_Byte(MPU_GYRO_ZOUTH_REG);
	// buf[5]=MPU_Read_Byte(MPU_GYRO_ZOUTH_REG+1);
	
		*gx=((u16)buf[0]<<8)|buf[1];  
		*gy=((u16)buf[2]<<8)|buf[3];  
		*gz=((u16)buf[4]<<8)|buf[5];
 	
    return 0;
}
//得到加速度值(原始值)
//gx,gy,gz:陀螺仪x,y,z轴的原始读数(带符号)
//返回值:0,成功
//    其他,错误代码
u8 numx,numy;
u8 MPU_Get_Accelerometer(short *ax,short *ay,short *az)
{
   u8 buf[6];
	// buf[0]=MPU_Read_Byte(MPU_ACCEL_XOUTH_REG);
	// buf[1]=MPU_Read_Byte(MPU_ACCEL_XOUTH_REG+1);
	// buf[2]=MPU_Read_Byte(MPU_ACCEL_YOUTH_REG);
	// buf[3]=MPU_Read_Byte(MPU_ACCEL_YOUTH_REG+1);
	// buf[4]=MPU_Read_Byte(MPU_ACCEL_ZOUTH_REG);
	// buf[5]=MPU_Read_Byte(MPU_ACCEL_YOUTH_REG+1);
	MPU_Read_Len(0, MPU_ACCEL_XOUTH_REG, 6, buf);
		*ax=((u16)buf[0]<<8)|buf[1];  
		*ay=((u16)buf[2]<<8)|buf[3];  
		*az=((u16)buf[4]<<8)|buf[5];
    return 0;
}
 

//陀螺仪转换角速度 
void Gyro_Change(void)
{
	//wrong code - case 1
	//Gyro_Pitch = ((float)gyrox  - gyro_offset_x) / 8.667f; //换算角速度 -28->初始值  角速度=原始值/分辨率
	//Gyro_Roll = ((float)gyroy - gyro_offset_y) / 8.667f; //换算角速度 -7->初始值  角速度=原始值/分辨率
	//Gyro_Yaw = ((float)gyroz - gyro_offset_z) / 8.667f; //换算角速度 4->初始值  角速度=原始值/分辨率

	//case 2
	// +/-2000(d/s) 32767 -> 2000 
	// LSB = 32767/2000 = 16.3835
	// 16.3835f = 32768/2000
	
/*
65.534 -> 500
131.068 -> 250
32.767 -> 1000
16.3835 -> 2000 
*/
	float factor = 16.3835f;	
	struct timeval cur_time;
	gettimeofday(&cur_time, NULL);  
	double cur_Seconds = cur_time.tv_sec + cur_time.tv_usec/1000000.0;
	double delay = cur_Seconds - last_Seconds;
	//printf("delay %f\n", delay);
	last_Seconds = cur_Seconds;

	Gyro_Pitch = (gyrox  - gyro_offset_x) / factor; //换算角速度 -28->初始值  角速度=原始值/分辨率
	Gyro_Roll = (gyroy - gyro_offset_y) / factor; //换算角速度 -7->初始值  角速度=原始值/分辨率
	Gyro_Yaw = (gyroz - gyro_offset_z) / factor; //换算角速度 4->初始值  角速度=原始值/分辨率

	double tmp = (Gyro_Pitch) * delay;
	if ((tmp > YAW_min_move) || (tmp < -YAW_min_move))
	{
		Angle_Pitch += (tmp);
	}

	tmp = (Gyro_Roll) * delay;
	if ((tmp > YAW_min_move) || (tmp < -YAW_min_move))
	{
		Angle_Roll += (tmp);
	}

	tmp = (Gyro_Yaw) * delay;
	if ((tmp > YAW_min_move) || (tmp < -YAW_min_move))
	{
		Angle_Yaw += (tmp);
	}

	printf("pitch\t%f\t\troll\t%f\t\tyaw\t%f\n", Angle_Pitch, Angle_Roll, Angle_Yaw);
}
 
//加速度计转加速度
void Acc_Change(void)
{
	Acc_Pitch = G * (((float)aacx - aac_offset_x)/ 16384.0f);//+-1g的数据分辨率为 +32768/1=16384L LSB/g 单位 m/s2
	Acc_Roll = G * (((float)aacy - aac_offset_y)/ 16384.0f);//+-1g的数据分辨率为 +32768/1=16384L LSB/g 单位 m/s2
	Acc_Yaw = G * (((float)aacz - aac_offset_z) / 16384.0f);//+-1g的数据分辨率为 +32768/1=16384L LSB/g 单位 m/s2
//    printf("Acc_Pitch = %.1f Acc_Roll = %.1f Acc_Yaw = %.1f   ",Acc_Pitch,Acc_Roll,Acc_Yaw); 
   //printf("x\t%f\t\ty\t%f\t\tz\t%f\n",Acc_Pitch,Acc_Roll,Acc_Yaw); 
}
 
//角度获取
double YAW_sum = 0.0f;
//57.2975 = 180/pi
void Angle_Get(void)
{
	float Acc_x, Acc_y, Acc_z;
	Acc_x = Acc_Pitch;
	Acc_y = Acc_Roll;
	Acc_z = Acc_Yaw;
	//case 1
	// Angle_Pitch = -atan2((float)aacy, (float)aacz) * 57.2974f;
	// Angle_Roll = -atan2((float)aacx, (float)aacz) * 57.2974f;

	//case 2
	//Angle_Pitch = -atan2((float)Acc_Roll, (float)Acc_Yaw) * 57.2974f;
	//Angle_Roll = -atan2((float)Acc_Pitch, (float)Acc_Yaw) * 57.2974f;

	//case 3
	//y轴与其水平面投影线之间的夹角，当载体抬头的时候角度为正。
	//z轴与y轴所在铅垂面之间的夹角,右倾的时候角度为正。
	//double my_pitch = atan2(Acc_y, Acc_z) * 57.2974f;
	//double my_roll = atan2(-Acc_x, Acc_z) * 57.2974f;
	
	int factor = 1;
	if (Acc_z < 0)
		factor = -1;
	
	double my_pitch = atan2(Acc_y, sqrt(Acc_z*Acc_z + Acc_x*Acc_x)) * 57.2974f;
	double my_roll = atan2(-Acc_x, factor*sqrt(Acc_z*Acc_z + Acc_y*Acc_y)) * 57.2974f;

	printf("pitch\t%f\t\troll\t%f\n",my_pitch, my_roll);

	// YAW_sum = (Gyro_Yaw) * 0.01;
	// if ((YAW_sum > YAW_min_move) || (YAW_sum < -YAW_min_move))
	// {
	// 	Angle_Yaw += (YAW_sum);
	// }
	

}
 
 
//校准6050
void MPU_init_Offset(void)
{
	printf("init offset\n");
	for (u8 i = 0; i < 100; i++)
	{
		MPU_Get_Gyroscope(&gyrox, &gyroy, &gyroz);
		gyro_offset_x += ((float)gyrox) / 100.0f;
		gyro_offset_y += ((float)gyroy) / 100.0f;
		gyro_offset_z += ((float)gyroz) / 100.0f;
		usleep(1000);
	}
	printf("gyro offset dat x y z = %.1f,%.1f,%.1f \n",  gyro_offset_x, gyro_offset_y, gyro_offset_z); 
	for (u8 i = 0; i < 100; i++)
	{
		MPU_Get_Accelerometer(&aacx, &aacy, &aacz);
		aac_offset_x += ((float)(aacx)) / 100.0f;
		aac_offset_y += ((float)aacy) / 100.0f;
		aac_offset_z += ((float)aacz-16384.0f) / 100.0f;
		usleep(1000);
	}
	printf("aac offset dat x y z = %.1f,%.1f,%.1f \n",  aac_offset_x, aac_offset_y, aac_offset_z);
 
}
 
 